#include "preprocess/voxel_preprocessor.h"

#include <random>

VoxelProcessor::VoxelProcessor() noexcept {}

VoxelProcessor::~VoxelProcessor() {}

bool VoxelProcessor::operator()(
    FixedArray<LidarData, kMaxLidarNum> const& lidar_data,
    VoxelPreProcessParameters const& parameters, std::vector<float>& voxel_data,
    std::string& err) noexcept {
  // 参数合法检查
  if (parameters.mp.size() < 2) {
    err = "parameter mp  illegal";
    return false;
  }
  if (parameters.voxel_size.size() < 3) {
    err = "parameter voxel size  illegal";
    return false;
  }
  if (parameters.net_range.size() < 6) {
    err = "parameter net range  illegal";
    return false;
  }

  auto const& voxelSize{parameters.voxel_size};
  auto const& netRange{parameters.net_range};
  auto const& m1{parameters.mp.at(0)};
  auto const& m2{parameters.mp.at(1)};
  auto const& maxPointsPerVoxel{parameters.max_points_per_voxel};

  auto netLength =
      static_cast<int32_t>((netRange[3] - netRange[0]) / voxelSize[0]);

  auto netWidth =
      static_cast<int32_t>((netRange[4] - netRange[1]) / voxelSize[1]);

  // 初始化
  auto bRet = true;
  auto initFunc{[&]() {
    // 资源分配
    num_points_per_voxel_ = std::vector<std::vector<std::int16_t>>(
        netWidth * m1, std::vector<std::int16_t>(netLength * m2, 0));

    random_array_ptr_ = std::make_shared<std::vector<uint32_t>>();
  }};

  std::call_once(init_flag_, initFunc);

  if (!bRet) {
    return false;
  }

  int32_t maxPointNumber = 0;
  for (std::uint32_t i{0}; i < lidar_data.GetNumberFilled(); ++i) {
    if (lidar_data.Get(i).is_valid) {
      auto const& lidarDataNumber{lidar_data.Get(i).point_cloud.GetPointNum()};
      if (lidarDataNumber > maxPointNumber) {
        maxPointNumber = lidarDataNumber;
      }
    }
  }

  if (maxPointNumber > random_array_ptr_->size()) {
    // 生成随机数组
    random_array_ptr_->resize(maxPointNumber);
    for (std::uint32_t i{0}; i < maxPointNumber; ++i) {
      random_array_ptr_->at(i) = i;
    }
    std::shuffle(
        random_array_ptr_->begin(), random_array_ptr_->end(),
        std::default_random_engine(
            std::chrono::system_clock::now().time_since_epoch().count()));
  }

  // voxel
  float vx{voxelSize[0] / 2 + netRange[0]};
  float vy{voxelSize[1] / 2 + netRange[1]};
  float vz{voxelSize[2] / 2 + netRange[2]};
  float z_coe{voxelSize[2] / 2};

  std::memset(voxel_data.data(), 0, voxel_data.size() * sizeof(float));

  std::vector<int16_t> coor;
  coor.resize(voxelSize.size());

  std::vector<int16_t> gridSize;
  gridSize.resize(voxelSize.size());

  for (std::uint16_t i{0U}; i < gridSize.size(); ++i) {
    gridSize[i] =
        round((netRange[gridSize.size() + i] - netRange[i]) / voxelSize[i]);
  }

  for (std::uint32_t i{0}; i < lidar_data.GetNumberFilled(); ++i) {
    auto const& lidarData{lidar_data.Get(i)};

    if (!lidarData.is_valid) {
      continue;
    }

    auto const& pointCloud = lidarData.point_cloud;
    auto const& pointCloudNum{pointCloud.GetPointNum()};

    for (size_t j{0}; j < random_array_ptr_->size(); ++j) {
      auto const& selectIndex{random_array_ptr_->at(j)};
      if (selectIndex < pointCloudNum) {
        auto& point{pointCloud.GetPoint(selectIndex)};
        coor[0] = floor((point.x - netRange[0]) / voxelSize[0]);
        if (coor[0] < 0 || coor[0] >= gridSize[0]) {
          continue;
        }
        coor[1] = floor((point.y - netRange[1]) / voxelSize[1]);
        if (coor[1] < 0 || coor[1] >= gridSize[1]) {
          continue;
        }
        coor[2] = floor((point.z - netRange[2]) / voxelSize[2]);
        if (coor[2] < 0 || coor[2] >= gridSize[2]) {
          continue;
        }

        if (num_points_per_voxel_[coor[0]][coor[1]] >= maxPointsPerVoxel)
          continue;

        std::int16_t c{num_points_per_voxel_[coor[0]][coor[1]]};

        std::int32_t tmp1{netLength * m1 * netWidth * m2};
        std::int32_t tmp2{(coor[1] * m1 + c % m1) * netWidth * m2 +
                          (coor[0] * m2 + c / m2)};
        auto x{(point.x - (coor[0] * voxelSize[0] + vx)) / voxelSize[0] * 2.0};
        auto y{(point.y - (coor[1] * voxelSize[1] + vy)) / voxelSize[1] * 2.0};
        auto z{(point.z - vz) / z_coe};
        auto intensity{(static_cast<float>(point.intensity) - 127.5) / 127.5};

        auto x_b{point.x >= 0 ? 1 : -1};
        auto y_b{point.y >= 0 ? 1 : -1};

        auto x_index{0 * tmp1 + tmp2};
        auto y_index{1 * tmp1 + tmp2};
        auto z_index{2 * tmp1 + tmp2};
        auto i_index{3 * tmp1 + tmp2};

        if (x_index > voxel_data.size()) {
          err = std::string("x_index larger than voxel_data size, ") +
                "x_index=" + std::to_string(x_index) +
                ", voxel_data size=" + std::to_string(voxel_data.size());
          return false;
        }

        if (y_index > voxel_data.size()) {
          err = std::string("y_index larger than voxel_data size, ") +
                "y_index=" + std::to_string(y_index) +
                ", voxel_data size=" + std::to_string(voxel_data.size());
          return false;
        }

        if (z_index > voxel_data.size()) {
          err = std::string("z_index larger than voxel_data size, ") +
                "z_index=" + std::to_string(z_index) +
                ", voxel_data size=" + std::to_string(voxel_data.size());
          return false;
        }

        if (i_index > voxel_data.size()) {
          err = std::string("i_index larger than voxel_data size, ") +
                "i_index=" + std::to_string(i_index) +
                ", voxel_data size=" + std::to_string(voxel_data.size());
          return false;
        }

        voxel_data[x_index] = static_cast<float>(x);
        voxel_data[y_index] = static_cast<float>(y);
        voxel_data[z_index] = static_cast<float>(z);
        voxel_data[i_index] = static_cast<float>(intensity);
        num_points_per_voxel_[coor[0]][coor[1]] += 1;
      }
    }
  }
  return true;
}